#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "ethercat.h"

#ifdef _WIN32
#include <windows.h>  // Use Sleep function for Windows system
#else
#include <unistd.h>   // Use usleep function for Linux system
#endif

#define SLAVE_POSITION 2  // Position of the slave
#define TARGET_POSITION 1000000  // Target position
#define PROFILE_VELOCITY 10000  // Profile velocity
#define PROFILE_ACCELERATION 500  // Profile acceleration
#define PROFILE_DECELERATION 500  // Profile deceleration

// Delay function
void delay_ms(int milliseconds) {
#ifdef _WIN32
    Sleep(milliseconds);  // Use Sleep for Windows, unit is milliseconds
#else
    usleep(milliseconds * 1000);  // Use usleep for Linux, unit is microseconds
#endif
}

int SV630N(char* ifname)
{
    // Initialize EtherCAT master
    if (ec_init(ifname)) {
        printf("EtherCAT initialization succeeded.\n");
    }
    else {
        printf("EtherCAT initialization failed.\n");
        return -1;
    }

    // Configure slaves
    if (ec_config_init(FALSE) > 0) {
        printf("EtherCAT configuration succeeded.\n");
    }
    else {
        printf("EtherCAT configuration failed.\n");
        ec_close();
        return -1;
    }

    // Wait for all slaves to enter OP state
    ec_statecheck(0, EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE);

    // Configure PP mode
    uint8_t modePP = 1;  // PP mode
    ec_SDOwrite(SLAVE_POSITION, 0x6060, 0x00, FALSE, sizeof(modePP), &modePP, EC_TIMEOUTRXM);  // Set mode to PP mode
    delay_ms(100);  // Delay 100 milliseconds
    printf("PP \n");

    uint32_t profileVelocity = PROFILE_VELOCITY;
    ec_SDOwrite(SLAVE_POSITION, 0x6081, 0x00, FALSE, sizeof(profileVelocity), &profileVelocity, EC_TIMEOUTRXM);  // Set profile velocity
    delay_ms(100);  // Delay 100 milliseconds

    // uint32_t profileAcceleration = PROFILE_ACCELERATION;
    // ec_SDOwrite(SLAVE_POSITION, 0x6083, 0x00, FALSE, sizeof(profileAcceleration), &profileAcceleration, EC_TIMEOUTRXM);  // Set profile acceleration
    // delay_ms(100);  // Delay 100 milliseconds

    // uint32_t profileDeceleration = PROFILE_DECELERATION;
    // ec_SDOwrite(SLAVE_POSITION, 0x6084, 0x00, FALSE, sizeof(profileDeceleration), &profileDeceleration, EC_TIMEOUTRXM);  // Set profile deceleration
    // delay_ms(100);  // Delay 100 milliseconds

    // Enable motor
    uint16_t controlWord = 0x0006;  // Prepare enable
    ec_SDOwrite(SLAVE_POSITION, 0x6040, 0x00, FALSE, sizeof(controlWord), &controlWord, EC_TIMEOUTRXM);
    delay_ms(100);  // Delay 100 milliseconds
    printf("6 \n");

    controlWord = 0x0007;  // Enable motor
    ec_SDOwrite(SLAVE_POSITION, 0x6040, 0x00, FALSE, sizeof(controlWord), &controlWord, EC_TIMEOUTRXM);
    delay_ms(100);  // Delay 100 milliseconds
    printf("7 \n");

    // Set target position
    int32_t targetPosition = TARGET_POSITION;
    ec_SDOwrite(SLAVE_POSITION, 0x607A, 0x00, FALSE, sizeof(targetPosition), &targetPosition, EC_TIMEOUTRXM);
    delay_ms(100);  // Delay 100 milliseconds

    // Start motion
    controlWord = 0x000F;  // Start motion
    ec_SDOwrite(SLAVE_POSITION, 0x6040, 0x00, FALSE, sizeof(controlWord), &controlWord, EC_TIMEOUTRXM);
    delay_ms(100);  // Delay 100 milliseconds
    printf("F \n");

    // Start motion
    controlWord = 0x001F;  // Start motion
    ec_SDOwrite(SLAVE_POSITION, 0x6040, 0x00, FALSE, sizeof(controlWord), &controlWord, EC_TIMEOUTRXM);
    delay_ms(100);  // Delay 100 milliseconds
    printf("1F \n");

    // Wait for motion completion
    uint16_t statusWord = 0;
    int size = sizeof(statusWord);
    do {
        ec_SDOread(SLAVE_POSITION, 0x6041, 0x00, FALSE, &size, &statusWord, EC_TIMEOUTRXM);
        delay_ms(100);  // Delay after each status read to avoid high CPU usage
        printf("status %d, size %d \n", statusWord, size);
    } while (!(statusWord & 0x0400));  // Wait for target reached flag

    printf("Motion completed.\n");

    // Close EtherCAT master
    ec_close();
    return 0;
}

char ifbuf[1024];
boolean printSDO = FALSE;
boolean printMAP = FALSE;

int main(int argc, char* argv[])
{
    ec_adaptert* adapter = NULL;
    printf("SOEM (Simple Open EtherCAT Master)\nSlaveinfo\n");

    if (argc > 1)
    {
        if ((argc > 2) && (strncmp(argv[2], "-sdo", sizeof("-sdo")) == 0)) printSDO = TRUE;
        if ((argc > 2) && (strncmp(argv[2], "-map", sizeof("-map")) == 0)) printMAP = TRUE;
        /* start slaveinfo */
        strcpy(ifbuf, argv[1]);
        SV630N(ifbuf);
    }
    else
    {
        printf("Usage: slaveinfo ifname [options]\nifname = eth0 for example\nOptions :\n -sdo : print SDO info\n -map : print mapping\n");
        /* Print the list */
        printf("Available adapters\n");
        adapter = ec_find_adapters();
        while (adapter != NULL)
        {
            printf("Description : %s, Device to use for wpcap: %s\n", adapter->desc, adapter->name);
            adapter = adapter->next;
        }
    }

    printf("End program\n");
    return (0);
}